/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/opInclude/operation_pallet_identify.h
 * @Description  : 托盘识别类定义 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-27 19:11:43
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once
#include "operation.h"
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include "timestamp.h"
#include <functional>
#include <array>
#include "geometry.h"
#include <agv_msgs/msg/recognition_result.hpp>
#include <agv_srvs/srv/pallet_recognition.hpp>
// 末端路径的起始点KEY值
constexpr char KEY_START_ID[] = "startId";
constexpr char KEY_END_ID[] = "endId";

//////////////////////////////////////////////////////////////////////////////
//   The interface of class "COperation".
class OperationPalletIdentify : public Operation
{

public:
    // 定义输入IO点类型
    // using INPUT_IO_TYPE = CDataTimeStamp<agv_msgs::msg::PairTypeInt>;

    // 构造函数
    explicit OperationPalletIdentify(const agv_msgs::msg::Action &action);

    // 析构函数
    virtual ~OperationPalletIdentify() = default;

    // 初始化相关数据
    virtual bool Init() override;

    // 准备开始操作
    virtual bool ReadyStart() override;

    // 定义如何开始操作
    virtual bool StartAction() override;

    // 定义如何结束操作
    virtual bool EndAction() override;

    // 定义如何运行操作
    virtual bool Work() override;

public:
    enum class ExeResult {
        Ready = 0,
        Ok = 1,
        Failed = 2
    };
    // 获取原始地图起始点位姿
    // bool GetRawMapPath(int start, int end, std::array<CPosture, 2> &result);
    std::function<bool(int, int, std::array<Posture, 2> &)> GetRawPathCallBack;
    // 修正双向地图路段
    // bool ModifyDoubleMapPath(int start, int end, std::array<CPnt, 4> bezierPnts);
    // std::function<bool(int, int, std::array<CPnt, 4>)> ModifyDoublePathCallBack;
    std::function<bool(int, int, std::array<Point, 4>)> ReplacePalletEndPathCallBack;

    // 触发执行的当前ID;
    static int exe_id_;

    // 请求和回复类型
    using SrvType = agv_srvs::srv::PalletRecognition;
    using ReqType = agv_srvs::srv::PalletRecognition::Request;
    using ResponseType = agv_srvs::srv::PalletRecognition::Response;

private:
    // 校验参数是否存在起始点等字段
    bool ValidateParam();

    // 识别触发
    bool Recognize(int mode, int id, const Posture& start_pose, float center_depth);

    // 转换成geometrypose
    geometry_msgs::msg::Pose2D ToGeometryPose2D(const Point& point);

    //
    geometry_msgs::msg::Pose2D ToGeometryPose2D(const Posture& pos);

    //转换成CPnt
    Point FromGeometryPose2D(const geometry_msgs::msg::Pose2D& pose);

private:
    // 起点ID号
    int start_id_;
    // 终点ID
    int end_id_;
    // 原始路径起始点位姿
    std::array<Posture, 2> raw_pos_;
    // 触发识别客户端
    rclcpp::Client<agv_srvs::srv::PalletRecognition>::SharedPtr client_;
    // 托盘深度
    float work_depth_;
    // 执行的结果
    ExeResult result_;
};
